﻿/*
 * ©2009-2010 XELF
 * http://xelf.info
 */

using System;

namespace XELF.Framework {

	public partial struct Quaternion {
		/// <summary>
		/// 回転行列からクォータニオン
		/// </summary>
		/// <param name="rotation"></param>
		/// <param name="result"></param>
		public static void CreateFromRotationMatrix(ref Matrix rotation, out Quaternion result) {
			float trace = rotation.M11 + rotation.M22 + rotation.M33;
			if (trace > float.Epsilon) {
				float s = MathHelper.Rsqrt(1.0f + trace);
				result.W = 0.5f / s;
				s *= 0.5f;
				result.X = (rotation.M23 - rotation.M32) * s;
				result.Y = (rotation.M31 - rotation.M13) * s;
				result.Z = (rotation.M12 - rotation.M21) * s;
			} else {
				if (rotation.M11 > rotation.M22 && rotation.M11 > rotation.M33) {
					float s = MathHelper.Rsqrt(1.0f + rotation.M11 - rotation.M22 - rotation.M33);
					result.X = 0.5f / s;
					s *= 0.5f;
					result.W = (rotation.M23 - rotation.M32) * s;
					result.Y = (rotation.M12 + rotation.M21) * s;
					result.Z = (rotation.M13 + rotation.M31) * s;
				} else if (rotation.M22 > rotation.M33) {
					float s = MathHelper.Rsqrt(1.0f - rotation.M11 + rotation.M22 - rotation.M33);
					result.Y = 0.5f / s;
					s *= 0.5f;
					result.W = (rotation.M31 - rotation.M13) * s;
					result.X = (rotation.M12 + rotation.M21) * s;
					result.Z = (rotation.M23 + rotation.M32) * s;
				} else {
					float s = MathHelper.Rsqrt(1.0f - rotation.M11 - rotation.M22 + rotation.M33);
					result.Z = 0.5f / s;
					s *= 0.5f;
					result.W = (rotation.M12 - rotation.M21) * s;
					result.X = (rotation.M13 + rotation.M31) * s;
					result.Y = (rotation.M23 + rotation.M32) * s;
				}
			}
		}
	}

}
